using System;
using System.Collections.Generic;
using System.Text;
using System.Text.RegularExpressions;

namespace Blimpbots.Control {

    public class Blimp {
        // this is a port of the BlimpBot class from C++
        private bool valid;
        private System.IO.Ports.SerialPort serialPort;
        private string name;

        double lastVoltage = 0;
        DateTime lastVoltageTime = DateTime.MinValue;
        bool expectingVoltage = false;

        public double Voltage { get { return lastVoltage; } }
        public TimeSpan VoltageAge { get { return DateTime.Now.Subtract(lastVoltageTime); } }

        public Blimp(string port, string name) {

            this.name = name;

            serialPort = new System.IO.Ports.SerialPort();
            serialPort.PortName = port;
            serialPort.BaudRate = 9600;
            serialPort.Parity = System.IO.Ports.Parity.None;
            serialPort.DataBits = 8;
            serialPort.StopBits = System.IO.Ports.StopBits.One;
            serialPort.Handshake = System.IO.Ports.Handshake.None;

            serialPort.ReadTimeout = 1000;
            serialPort.WriteTimeout = 100;
            
            Console.WriteLine("opening " + serialPort.PortName + "..");
            valid = true;
            try {
                if (serialPort.IsOpen) {
                    Console.WriteLine("port already open, trying to close.\n");
                    serialPort.Close();
                }
                serialPort.Open();
                serialPort.DataReceived += new System.IO.Ports.SerialDataReceivedEventHandler(serialPort_DataReceived);
            }
            catch (System.IO.IOException) {
                Console.WriteLine("can't connect!");
                valid = false;
            }

            Console.WriteLine("opened.");
        }

        void serialPort_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
        {
            if(!expectingVoltage) return;
            expectingVoltage = false;

            System.IO.Ports.SerialPort s = (System.IO.Ports.SerialPort)sender;
            try {
                string v = this.serialPort.ReadExisting();

                int idx = v.IndexOf("\n");
                if (idx != -1)
                {
                    int idx2 = v.IndexOf(")");
                    if (idx2 != -1)
                    {
                        v = v.Substring(idx + 2, idx2 - idx - 1);
                    }
                }

                /* Check if result contains voltage */
                Regex r = new Regex(@"(\d+)");
                Match m = r.Match(v);

                if (m.Success)
                {

                    if (m.Groups.Count > 1)
                    {
                        string val = m.Groups[1].Value;
                        int adcValue;
                        if (Int32.TryParse(val, out adcValue))
                        {
                            lastVoltage = 3.0 / ((double)adcValue / 1024);
                            lastVoltageTime = DateTime.Now;
                        }
                    }
                }
            }
            catch { Console.WriteLine("Serial Port read error!\n"); }
        }

        public bool setForwardDirection(int fan) {
            Console.WriteLine("Blimp " + name + " fan " + fan + " forward.");
            if (fan == 1) {
                return writePort("z");
            }
            else if (fan == 2) {
                return writePort("x");
            }
            else if (fan == 3) {
                return writePort("c");
            }
            else {
                Console.WriteLine("ERROR: invalid fan number.");
                return false;
            }
        }

        public void requestVoltage()
        {
            expectingVoltage = true;
            writePort("v");
        }

        public bool setReverseDirection(int fan) {
            Console.WriteLine("Blimp " + name + " fan " + fan + " reverse.");
            if (fan == 1) {
                return writePort("a");
            }
            else if (fan == 2) {
                return writePort("s");
            }
            else if (fan == 3) {
                return writePort("d");
            }
            else {
                Console.WriteLine("ERROR: invalid fan number.");
                return false;
            }
        }
        /// <summary>
        /// Set LED status
        /// </summary>
        /// <param name="on">Are LEDs on at all?</param>
        /// <param name="top">Which LED to use</param>
        public void SetLEDs(bool on, bool top)
        {
            Console.WriteLine("setting LED " + top.ToString() + " to " + on.ToString());
            if (on) writePort("}");
            else writePort("{");

            if (top) writePort("]");
            else writePort("[");
        }

        public bool setSpeed(int fan, int speed) {
            Console.WriteLine("Blimp " + name + " fan " + fan + " speed " + speed);
            if (fan == 1) {
                if (!writePort("q")) {
                    return false;
                }
            }
            else if (fan == 2) {
                if (!writePort("w")) {
                    return false;
                }
            }
            else if (fan == 3) {
                if (!writePort("e")) {
                    return false;
                }
            }
            else {
                Console.WriteLine("ERROR: invalid fan number.");
                return false;
            }

            //System.Threading.Thread.Sleep(1000);
            // speed is 0-10
            if (speed == 0) {
                return writePort("`");
            }
            else if (speed == 1) {
                return writePort("1");
            }
            else if (speed == 2) {
                return writePort("2");
            }
            else if (speed == 3) {
                return writePort("3");
            }
            else if (speed == 4) {
                return writePort("4");
            }
            else if (speed == 5) {
                return writePort("5");
            }
            else if (speed == 6) {
                return writePort("6");
            }
            else if (speed == 7) {
                return writePort("7");
            }
            else if (speed == 8) {
                return writePort("8");
            }
            else if (speed == 9) {
                return writePort("9");
            }
            else if (speed == 10) {
                return writePort("0");
            }
            else {
                Console.WriteLine("ERROR: invalid speed.");
                writePort(" "); // any speed not above will cause a no-op, 
                // but we must finish the command sequence
                return false;
            }
        }

        private bool writePort(String s) {
            if (!valid) {
                Console.WriteLine("blimp " + name + " unreachable!");
                return false;
            }
            try
            {
                serialPort.Write(s);
            }
            catch
            {
                Console.WriteLine("ERROR: can't reach port " + name);
                valid = false;
                return false;
            }
            return true;
        }

        public bool setFPSpeed(int fan, double speed) {
            double absSpeed;
            if (speed < 0) {
                absSpeed = -1 * speed;
            }
            else {
                absSpeed = speed;
            }
            if (speed > 0) {
                setReverseDirection(fan);
            }
            else {
                setForwardDirection(fan);
            }

        //    absSpeed += .99999999; // round up

            if (absSpeed < 1) {
                return setSpeed(fan, 0);
            }
            else if (absSpeed < 2) {
                return setSpeed(fan, 1);
            }
            else if (absSpeed < 3) {
                return setSpeed(fan, 2);
            }
            else if (absSpeed < 4) {
                return setSpeed(fan, 3);
            }
            else if (absSpeed < 5) {
                return setSpeed(fan, 4);
            }
            else if (absSpeed < 6) {
                return setSpeed(fan, 5);
            }
            else if (absSpeed < 7) {
                return setSpeed(fan, 6);
            }
            else if (absSpeed < 8) {
                return setSpeed(fan, 7);
            }
            else if (absSpeed < 9) {
                return setSpeed(fan, 8);
            }
            else if (absSpeed < 10) {
                return setSpeed(fan, 9);
            }
            else {
                return setSpeed(fan, 10);
            }
        }

        public void StopMotors()
        {
            if (valid)
            {
                setSpeed(1, 0);
                setSpeed(2, 0);
                setSpeed(3, 0);
            }
        }

        public void destruct() {
             Console.WriteLine("destructing blimp\n");
            if (valid) {
                StopMotors();
                serialPort.Close();
            }
        }

    }

   
}
